import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import Pose
from std_msgs.msg import Header
import numpy as np

class PosePublisher(Node):
    def __init__(self):
        super().__init__('pose_publisher')
        self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, 'pose_with_covariance', 10)
        self.timer = self.create_timer(1.0, self.publish_pose)  # 每1秒发布一次
        self.get_logger().info('Pose Publisher Node has started.')

    def publish_pose(self):
        # 创建 PoseWithCovarianceStamped 消息
        msg = PoseWithCovarianceStamped()
        
        # 设置消息头
        msg.header = Header()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'map'  # 设置参考坐标系
        
        # 设置 Pose
        msg.pose.pose = Pose()
        msg.pose.pose.position.x = 1.0  # 设置位置
        msg.pose.pose.position.y = 2.0
        msg.pose.pose.position.z = 0.0
        msg.pose.pose.orientation.x = 0.0  # 设置方向
        msg.pose.pose.orientation.y = 0.0
        msg.pose.pose.orientation.z = 0.0
        msg.pose.pose.orientation.w = 1.0
        
        # 设置协方差矩阵（6x6）
        msg.pose.covariance = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
                               0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
                               0.0, 0.0, 0.1, 0.0, 0.0, 0.0,
                               0.0, 0.0, 0.0, 0.1, 0.0, 0.0,
                               0.0, 0.0, 0.0, 0.0, 0.1, 0.0,
                               0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
        
        # 发布消息
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing PoseWithCovarianceStamped message')

def main(args=None):
    rclpy.init(args=args)
    node = PosePublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
